#include "KALMAN_FILTER.h"
#include <math.h>
#include "OLED.h"

/**
  * @brief  卡尔曼滤波器初始化
  * @param  kf         滤波器实例指针
  * @param  Q_angle    过程噪声-角度（推荐值 0.001~0.003）
  * @param  Q_bias     过程噪声-零偏（推荐值 0.001~0.003）
  * @param  R_measure  测量噪声（推荐值 0.03~0.1）
  */
void Kalman_Init(KalmanFilter *kf, float Q_angle, float Q_bias, float R_measure) {
    kf->angle = 0.0f;        // 初始角度设为0
    kf->bias = 0.0f;         // 初始零偏设为0
    kf->Q_angle = Q_angle;   
    kf->Q_bias = Q_bias;
    kf->R_measure = R_measure;
    
    // 初始协方差矩阵（较大不确定性）
    kf->P[0][0] = 1.0f;      // 角度方差
    kf->P[0][1] = 0.0f;      // 角度-零偏协方差
    kf->P[1][0] = 0.0f;      // 零偏-角度协方差
    kf->P[1][1] = 1.0f;      // 零偏方差
}

/**
  * @brief  卡尔曼滤波器更新（简化协方差运算）
  * @param  gyro_rate    陀螺仪角速度（单位：度/秒）
  * @param  accel_angle  加速度计计算的角度（单位：度）
  * @param  dt           采样时间（单位：秒）
  */
void Kalman_Update(KalmanFilter *kf, float gyro_rate, float accel_angle, float dt) {
    /*----- 1. 预测步骤 -----*/
    // 状态预测：角度 = 上一角度 + (角速度 - 零偏) * dt
    kf->angle += (gyro_rate - kf->bias) * dt;
    
    // 协方差预测（简化计算，忽略部分交叉项）
    kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
    kf->P[0][1] -= dt * kf->P[1][1];
    kf->P[1][0] -= dt * kf->P[1][1];
    kf->P[1][1] += kf->Q_bias * dt;

    /*----- 2. 测量更新 -----*/
    // 计算新息（测量残差）
    float y = accel_angle - kf->angle;
    
    // 计算卡尔曼增益（简化分母计算）
    float S = kf->P[0][0] + kf->R_measure;
    float K0 = kf->P[0][0] / S;      // 角度增益
    float K1 = kf->P[1][0] / S;      // 零偏增益
    OLED_Printf(70,2,OLED_6X8,"%.2f%.2f",K0,K1);

    /*----- 3. 状态修正 -----*/
    kf->angle += K0 * y;
    kf->bias  += K1 * y;


    /*----- 4. 协方差修正（简化）-----*/
    float P00 = kf->P[0][0];
    float P01 = kf->P[0][1];
    
    kf->P[0][0] -= K0 * P00;
    kf->P[0][1] -= K0 * P01;
    kf->P[1][0] -= K1 * P00;
    kf->P[1][1] -= K1 * P01;
}
